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ABSTRACT 


Two Extended Kalman filter routines, one uSing a one-step 
estimation/prediction and the other a sequential approach, 
were developed and compared to provide real time estimates 
of target positions on the three dimensional underwater track- 
ing range at Naval Underwater Weapons Engineering Station, 
Keyport, Washington. Inputs to the routines were acoustic 
pulse transit times from the target to receiving array ele- 
ments which are non-linear functions of the position coordin- 
ates. These inputs were linearized and the filter gains cal- 
culated on-line. Simulated runs were conducted for tracks in 
the area of one hydrophone array and for tracks that transited 
through multiple arrays. It was found that the sequential 
estimate routine exhibited better verformance in recovering 
from transients caused by random measurement noise or target 


movement. 
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i LN eCDUCT LON 


The Naval Underwater Weapons Engineering Station, Keyport, 
Washington currently operates two three-dimensional under- 
Water tracking ranges with the capability of acoustically 
tracking torpedoes. Underwater tracking employs an acoustic 
device installed in the object to be tracked. This device 
transmits timed acoustic pulses which are received by bottom 
mounted hydrophone arrays and then relayed via cable to a 
computer at the observation site which calculates the position 
@e tie ob Ject for each pulse and plots its path. 

The measured data, which is the time elapsed from trans- 
mission of a pulse until its receipt at the hydrophone array, 
1s corrupted by noise due to the combined effects of environ- 
mental factors and the measurement instruments. 

These noisy tracks are later analyzed, and measurements 
judged most inaccurate on the basis of total track statistics 
are removed in order to obtain a smooth representation of the 
ecack. 

As stated in Reference 1, the computer system at the 
Dabob range of the station will be up-graded, and will consist 
of three MODCOMP IV computers. by DATACOM, Inc. A software 
conversion project will take place with applications software 
being developed for the new computers. The bulk of this 
development will consist of converting the current tracking 


programs and other related programs to FORTRAN. 





An opportunity exists for expanding the real-time capa- 
bility of the system by applying a Kalman filter routine 
which can take as an input the transit times of the acoustic 
pulses, and produce the best estimate of the position of the 


tracked object at a particular time. 





IIT. THREE DIMENSIONAL RANGE DESCRIPTION 


The three dimensional range described in Reference 2 is 
an acoustic system capable of determining the trajectory of 
Suitably instrumented underwater objects in the vicinity of 
a transducer array placed at the bottom of the bay. The 
tracked unit (torpedo) carries a synchronous clock and an 
acoustic transducer. A hydrophone array defines a rectan- 
gular coordinate system to which measurements are referred. 
Positional information 1s obtained from the transit times of 
a4 periodic pulsed acoustic signal traveling from the torpedo 
to four independent hydrophones located on each array. The 
geometry of the hydrophones and the coordinate’ system is 
illustrated in Figure 1. On each array, the four hydrophones 
Ras Ry Ry and Ry are on four adjacent vertices of a cube. 
Each hydrophone is separated by a distance d= 30 feet along 
the edges. The origin of the coordinate system is at the 
center of the cube. 

The transit times of the acoustic pulse from the tracked 
object to each of the four independent hydrophones can be 


expressed as follows: 


L/veL | (x+a/2)7 + (y+a/2)? + (2+d/2)* 
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FIGURE 1: The Geometry Used in the Calculation 
Gt POs tienda! Coordinates. (The.ori2in 
is at the center of the dashed cube). 
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Where VEL equals the velocity of propagation of sound in 
water and C, X, Y and Z are the four hydrophones on each array. 
It is essential as part of this fundamental calculation to 
know when the tracked object emits an acoustic pulse in order 
to measure the transit times to the hydrophones. For submerged 
objects two stable crystal-controlled clocks, one in the track- 
ed unit and the other at the computer are used. Prior toa 
fom the two clocks are synchronized by radio. 

The range is a high frequency, short-baseline facility 
with the acoustic tracking pulses emitted at 75 kHz. The 
range layout consists of six in-line hydrophone arrays spaced 


2000 yards apart. 
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fia THEORY 


In Reference 3, a Kalman filter application was used 
assuming a linear system and filtering on the corrupted X, Y 
and Z2 positions that the computer had alreadycalculated from 


the received transit times of the acoustic signals. 


A. THE EXTENDED KALMAN FILTER 

Since the transit times were readily available and are non- 
linear functions of position, these equations can be linearized 
and Kalman filter theory applied using the extended Kaiman 
filter. This procedure produces a real-time system, filtering 


Ak Te and. LF 


ee pe Y a 


necessity of converting these times to positions. 


on the corrupted transit times T without the 


For tracking, a £1i£fth order state vector was chosen: 


tx 
{| 
NMieK mK XxX 


The target was aaeuried to Maintain constant depth and any 
velocity in the Z2 direction (2) was considered a random exci- 
mation. ; 

The states were characterized by the following difference 
equation: 


X(K+t1) = ® x (K) + 5 W (K) 


and the noisy measurement equation: 


iy? 





72(K) = M(X) + V(RK) 
where 
X (K) is the N-dimensional state vector at time K 


W(K) is the M-dimensional random forcing input 
: at time K 


Z(K) is the J-dimensional measurement vector at 
* time K 
V (K) is the J-dimensional random noise vector at 


time K and noise is assumed white and zero- 
mean Gaussian 


6 and [— are constant matrices 


M (xX) is a matrix of the non-linear measurement 
if equations which are a function of the states. 


The estimator equations are given by: 


X(K/K) 


~ 


x (K/K-1) + G(K)* w(K) -—- M(X) *¥% (K/K-1) 
and 


P(K/K) = E SG (i (Ke) | SOG AGaale) 


™ ~e 


where 


X(K/K) is the estimate of the state at time K given 
~ K measurements 


P(K/K) is the covariance of estimation error matrix 
i at time K 


G(K). is the Kalman filter gain at time K 
which is defined as: 


ee Ge Ke) H(K) "|B (8) P(K/K-L)H(K)™ 7 R(x) | 


R(K) is the covariance of random measurement noise 
; matrix 

H(K) is the matrix used to linearize the non-linear 
: measurement equations. It is defined as: 
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a eer | (k/ R41) 


The measurement equation matrix M (=) is expanded in a 


Taylor series and linearized around the one step prediction 


based on the last estimate. Only first order terms are kept. 


The prediction equations are given by: 


where 


X(K+1/K) - 0X (K/K) +P W(K) 
P(K+1/K) = @P(K/K)$” + Q(K) 


Q(K) is the covariance of random excitation 
~ matrix found by: 


O(K) = © covc(wyrt 


~ 


and COV(W) is the covariance of the random 
forcing input or acceleration. 


B. THE SEQUENTIAL EXTENDED KALMAN FILTER 


The prediction and estimation equations just mentioned 


represent a system that is characterized by a one-step esti- 


mation and prediction for each set of new measurements. Some 


important points in this one step process must be stressed: 


al 


New estimates are only available after the prediction 
and estimation equations are calculated. 

A major part of the time period for the estimation 
calculation is spent in the gain equation because of 
the inversion of the (JXJ) matrix that is required, 


winerewd is the number of observations. 
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3. The one-step prediction and estimation for the exten- 
ded Kalman filter is based on a linearization about 
the predicted value over a period ty EIS) eal Wild ou 
Might be extended. 

When the measurements come from statistically independent 
sources, like the hydrophones in the torpedo tracking pro- 
blem, a sequential approach can be taken tO process each 
arrival time separately. 

If the measurements are assumed to occur simultaneously, 
they can be processed one at a time and the result of pro- 


cessing one measurement component is used in the following 


computation to process the next measurement component. 


Ls 





vs 
EXTENDED KALMAN F 


In the torpedo tracking problem, 


PROBLEM DEFINITION - TORPEDO TRACKING WITH THE 


ees 


the non-linear observa- 


tions are the four independent transit times from the tracked 


object to the hydrophones, T 


linear 








ik Z 
To VEL | (+a /2) 
1 2 
TY JEL | (x-a/2) 
M(%) = _ 
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Vi Je 
M(X) is expanded into a Tayl 
Sy : 
x 
M(X) = a X = 
| CK Kp) 


The linearizing H matrix 
information available at the 


ty 
X(K/K-1), and is used in the 


Co 


measurement matrix M(X) 


x! Ty 


is defined as: 


and T_. Thus the non- 
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X(K/K-1) 
is evaluated around the best 


time which is the prediction 


calculation of the gain G(K) and 


estimated covariance of error P(K/K) equations, 


The torpedo dynamics used for the tracking problem are 


assumed to be Ty/ses with extimations on five states X position, 
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Pavel@ctty, * pesltion, Y velocity and Z position (height of 
torpedo above hydrophone array). The mean of the random ex- 
Citation is assumed to be zero [E(W(K))=0], and this simpli- 


fies the estimate prediction equation to: 
X(K+1/K) = © X(K/K) 


Also the mean of the random ncise is assumed to be zero 
fe(v(x) )=0) In forming the measurement equation, the best 
guess of v(K) is used which is the mean producing: 

Ei i) 
Four measurements are taken every 1.31 seconds, which is one 
time slot, and with this sampling time the 1/2 plant has 


State transition (PHI) and gamma matrices equal to: 


1 1.31 0 0 
0 i 0 0 
6 = 0 0 ey aa aa le 
0 0 0 0 
0 0 0 " 
and 
86 0 0 
Leal 0 0 
r= 0 36 0 
0 1, ail 0 


A. THE SEQUENTIAL APPROACH 


In the sequential approach, the basic Kalman filter equations 


ney 





have been modified to circumvent the matrix inversion in the 
gain equation and to obtain a more accurate estimate. Calcu- 
lations are performed on each of the four independent transit 


a Tana T. for each 1.31 


times in the following order T xt Ty 


Cc! 
second time slot. 

The estimate of the states, K(K/K) , based on one time 
measurement is used as the prediction X(K/K-1) for the calcu- 
lations on the next measurement. 

In this manner only parts of the linearizing H matrix and 


gain matrices are used in each calculation. 


After the linearizing H matrix is formed from 


X(K/K-1) 


evaluated around the initial states X(1/0) for K=l1, the first 


gain column corresponding to the first time measurement T, is 


© 
calculated from: 
P(K/K-1)H- 
Sicon es 
Bi row: (8/K-1) 43 nowt Ri 
where i= 1 to J, and J is the number of observations. In 


the tracking problem J = 4 corresponding to the four measured 
times. 

Thus, the first row of the H Matrix is used to calculate 
the first column of the gain matrix with both corresponding 


to the first measured time Tas 
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o~ 


Next, an estimate of the particular observation time M(X) 


~ 


is calculated 


MN 


= 
Et 


a, — [xa /2) 24 (v4a/2) 24 (2+a/2) 7 


* 1 2 2 2) 3 
T.. TEL [(x-a/2) + (Y+d/2) +(24a/2) >| 
M(X) = = 
‘i a ik z Z 2 be 
Ty JEL (4a /2) + (Y¥-d/2) +(24€/2)7 : 
a il Z 2 2) 5 
2, | VEL | (xea/2) + (Y+d/2) ge aaa 


Using the predicted values of X, Y, Z from X(K/K-1). 


The difference between the observed transit time Zs and 


the estimated transit time T. forms the residual Ly eP which 


is used in the estimate equation. 


X, = X(K/K-1) + G,COL Zo rpe | 


This equation gives an estimate of the states based on 


one measurement. 


Next, the covariance of estimation error is calculated 


based on one measurement using: 


Pa E - °scon*sxow im 


aL 








where 


I equals the identity matrix 


is the theoretical covariance of estimation 
error from the previous measurement or if 
i=l, the prediction P(K/K-1) 


After the first iteration, X, becomes X(K/K-1l) and P 


ai 1 


becomes P(K/K-1) for the second iteration which calculates 
the estimate of the states based on the second measurement 


Ty 
After four iterations (1=4), X becomes the estimate for 


the time slot, X(K/K) and P, becomes the updated covariance 


4 
of error P(K/K). 
Then the predictions for the next time slot are calcula- 


ted using: 


X(K+1/K) X(K/K) 


Il 
1H 


and 


P (K/K) ® + Q(K) 


II 
or 


P(K+1/K) 


The entire process is repeated for the next set of 
measurements forming a sequential, extended Kalman filter to 
produce real time estimates of the torpedo track. 

A listing of the FORTRAN program designed for the se~- 
guential extended Kalman filter is contained in Appendix B. 
The program is in modular form and well documented by comments 
for ease of implementation. All repetitive calculations and 
utility routines are separated into subroutines and are listed 


in Appendix D. 
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B. THE MATRIX INVERSION APPROACH 

All results obtained using the sequential, extended Kalman 
Filter were compared to results from a traditional extended 
Kalman filter using the equations delineated in the THEORY 
section. This includes an inversion of a 4x4 matrix in the 
gain equation. 


G(K) = P(K/K-1)H (K) "| (K) B (K/K-1) H(K) "+R (K) | 


The portion of the equation to be inverted is always symmetric 
and the IBM-360 library subroutine SINV was used to perform 
this operation. 


A listing of the FORTRAN program designed for the tradi- 


tional extended Kalman filter is contained in Appendix C. 


ZL 





V. TESTING AND SIMULATION 


Both the sequential and traditional Kalman filter routines 
were tested first using deterministic tracks at speeds of 5.0 
to 25.0 knots and no measurement noise with a single hydrophone 
array. The only errors allowed in the first phase of testing 
were in position and velocity in the initialization of the 
Filter. 

Computer generated tracks were tested in the first series 
of straight running, constant depth and constant velocity 
torpedoes. A variety of track scenerios were used transiting 


through multiple quadrants including: 


1. crossing north of the array 
Ze-eeeceOssing S©UEN Of the array 
3. inbound to the array 

4, outbound from the array 


5. crossing over top of the array 

All runs were made with a variety of initialization errors 
m2 position and velocity. 

In the second series of tests, white, zero-mean Gaussian 
noise was added to corrupt the observed transit times. 

The noise was added to the straight running, constant 
depth tracks. Before this series of tests could be conducted 
a gating scheme was designed to protect the filter from spurious 
erroneous time or positional data. 


In the third series of tests, a number of torpedo maneuvers 
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were added to the target tracks, One-third, two-thirds and 
one-G turns were used. These tracks were tested with and with- 
out noise. 

ary torpedo velocities were increased Bo, thes40 tov 50 .knor 
range in the fourth series of tests for straight running tracks 
with and without noise corruption. Maneuvers were then added 
to these higher velocity tracks. 

In the last series of tests, the handoff routine described 
at the end of this section was added to the filters and tracks 


that traversed through the areas of multiple arrays were 


tested. 


A. THE GATING SCHEME 

The operation of the filter may be adversely affected by 
large measurement noise. One error of a relatively large 
magnitude could invalidate the filtered output for many sub- 
sequent time slots. Before random measurement noise and ran- 
dom excitations could be added to the observed times for test- 
ing, a form of protection was designed to guard against cata- 
strophic failure. This protection is provided by establishing 
Himits of acceptability for each of the measurements. 

Measurement errors can occur because of many factors in- 
Cluding an error in the transit time of the acoustic pulse 
primarily due to the receipt of multipath signals from previous 
time slots that haye bounced off the surface, bottom or dif- 
ferent density layers, or large errors in the estimates of 


position or velocity. 
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A three-sigma gate was deisgned using the covariance of 
measurement noise (R) and the covariance of estimation error 
me (K/K) ) . 

For each calculation of a state estimate (X(K/K)), the 
largest acedthenedl covariance of error was used, either X, Y 
or Z, and converted to time in seconds using the average velo- 
city of sound in water for Dabob bay, 4860 ft/sec. The gate 


mAen was written Lor each time measurement i = 1 to 4: 


P (K/K) 
= largest 


The gate expands or decreases depending on the confidence level 
of the transit time and position estimate. If ZDIFF which is 
the difference between the actual transit time received and 

the predicted transit time to a particular hydrophone exceeds 
the gate, the measurement is considered unacceptable and the 
filter gain is set to Zero causing the filter to ignore the 


data and take the prediction of the states as the estimate. 
X(K/K) = X(K/K-1) 


For the sequential extended Kalman filter, because of the 
iterative aspect, a large erroneous time measurement zeros 
only the gain column for that Oe Mieuiar hydrophone causing 
only that hydrophone's data to be ignored. [In the traditional 
one-step filter, an erroneous input from any hydrophone zeros 


the entire gain matrix causing the filter to ignore data for 


the whole time slot. or 





B. MULTIPLE ARRAY TRACKING 

Initial tests were performed on tracks in the area of one 
array. In order to more closely simulate a typical run on the 
range, a scheme was designed to track a target through multiple 
arrays. 

First, a coordinate system was defined as shown in Figure 
2. The center of the coordinate system is geographically near 
the entrance to Dabob bay in the simulation. Array number 6 is 
the closest array to the coordinate center. Each hydrophone 
1S a particular array has an X, Y, Z position. In the simu- 
lation array 1 was at 36,000 feet from coordinate center and 
array 6 was 6000 feet. The C hydrophone was assumed to be the 
axis location of each array. Then each X position for the X 


hydrophone in each array was X.+30, each Y position for the Y 


e 
hydrophone was YQt30 and each Z position for the Z@ hydrophone 
was Zq+30. These 72 positions, an XYZ position for each of 4 
hydrophones in 6 arrays, were placed into a 6x12 matrix HYDRO 
and referenced throughout the routine. The geometry centered 
on each array was taken out of the problem and the target 


position was based on a central reference. 


The non-linear time equation became: 


f 2. 2 2 
T = 1/VEL (Xo K 9) “4 (¥-¥ 9) 74 (2-25) 


or YQ Or 4, is the position of a particular hydrophone 


and array being used. In the filter routine X, Y and Z were 


where X 


the predicted positions X(K/K-1), and the time equation was 
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used to calculate the estimate of the measurement times M(x). 
The decision parameter used to determine the switching from 
array to array was a straight handoff. If the predicted X 
position was greater than 3000 feet from the array in use, 
then an index (I8) was incremented and the next row of HYDRO 
was implemented. This placed into the routine the Xy¥ QZ 
positions of the hydrophones in the next array. The handoff 
can easily be utilized in real range operations, as the tran- 
Sit times from adjacent arrays are present at the computer for 
a particular time slot. 

For simulation, it was assumed that in all the arrays each 
ax1S pointed in the same direction. In range operations, the 
positions of the particular hydrophones referenced to the 


central coordinate system can be input into the matrix HYDRO 


to correct for OFF AXIS discrepancies. 
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VI. SIMULATION RESULTS 


A. SERIES ONE 

This series of tests included straight running, constant 
depth, constant velocity tracks with no noise. Various target 
speeds were tested ranging from 5.0 to 25.0 knots. 

The only induced .errors in this series of tests were in 
initial target position and velocity. Both the sequential 
and traditional filter routines effectively handled initial 
mOsition errors in the X AND/OR Y¥ direction from 0 to 25 feet 
and initial velocity errors from 0 to 10 ft/sec. The filter 
estimate was within 3 feet and 1 ft/sec in a maximum of 3 
time slots. In a number of worst case tests, initial posi- 
tion errors of up to 50 Roce and inittal Velocity errors of 
60 ft/sec and 80 ft/sec were used. Both filter routines had 
the estimate on track within seven time slots. 

Figure 3 is a geographical plot of a typical series one 
test using the sequential extended Kalman filter. The initiali- 
Zation of the filter was 14 feet off in X and 21 feet off in 
Mewith no error in Z. For a 25 knot target initial velocity 
errors were 3 ft/sec, Figures 4 through 6 depict the devia- 
tion in feet between the estimated and true positions, 

X, (K) - X,(K/K), where A = 1, 3 or 5. 

There was no great aelere renee in performance between the 

sequential and traditional filter routines in this series of 


tests. 
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FIGURE 5: X Deviation from True versus Time for a Straight 
Running Track without Noise in the Area of a 
Single Array 
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FIGURE 6: Y Deviation from True versus Time for a straight 
Running Track without Noise in the Area of a 
Single Array 
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B. SERIES TWO 

In the second series of tests random white Gaussian noise 
was added to the transit time measurements for straight running 
tracks. Target velocities varied from 5.0 to 25.0 knots. Ini- 
tial position errors ranged from 0 to 25 feet and initial velo- 
city errors from 0 to 10 ft/sec. 

Both the sequential and traditional routines performed 
well against the random noise. Estimate deviations from true 
track in KX and Y positions were not observed greater than 3 
feet after lock-on was acquired. Figures 7 and 8 depict the 
estimate deviation from true track for the sequential routine. 
Initial positions were 9 feet off in X, 13 feet in Y and 2 feet 


mm «2. 


fe SERIES THREE 

In this series of tests maneuvers were added to the torpedo 
tracks and the filter was tested with and without noise. 

Figure 9 is a geographical plot of a 25 knot target ina 
noiseless environment with 1/3-G turns at time slots 25 and 65. 
Figure 10 depicts the Z position versus the time of run. [In 
the initialization of the filter X was off 13 feet and Y was 
off 21 feet. Figure 11 depicts the deviation by the estimate 
from the true track in the X direction, for the traditional 
inversion approach. Figure 12 shows the deviation for the 
same track using the sequential extended Kalman filter. When 


the target initiated the turn at time slots 25 and 65, the 
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FIGURE 7: X Deviation from True versus Time for a straight 
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FIGURE 8: Y Deviation from True versus Time for a straight 
Running Track with Noise in the Area of a single 
Array -- Sequential Routine 
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FIGURE 11: X Deviation from True for a Track with 1/3-G 
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covariance of estimation error and thus the filter gains in- 
creased allowing more weight to be placed on the incoming 
data (transit times). In this manner the filter was able to 
adjust the velocities in the X and Y directions and force the 
estimate back on track. Results from both routines were com- 
parable as is shown in Figures 1l and 12. The routine using 
the sequential extended Kalman filter deviated a maximum of 
Four feet at the turn and was back on track within 3 time 
slots, while the traditional filter routine had a slightly 
greater deviation and required 2 more time slots to acquire 
lock again. Figure 13 shows the deviation of the estimate 
from true track in the Y direction for this same run for the 
sequential filter and Figure 14 for the traditional filter. 
Both estimates deviated a maximum of 3 feet at the turn and 
Meeacguired lock-on in 4 time slots with the sequential rou- 
tine reacting slightly better to the turns. 

Next, target tracks with 2/3 and 1-G turns were tested. 
Figure 15 is a geographic plot of this track with a 1-G turn 
at time slot 25 and a 2/3-G turn at time slot 65. Figure 16 
depicts the Z position vs time for this track. With an ini- 
Sialization error of 13 feet in X and 21 feet in Y, the maxi- 
mum deviation after lock-on was again at the turn points. 
Begure 17 depicts this deviation from true track in the xX 
direction, for the sequential. routine with a maximum deviation 
of approximately 5.5 feet for the 1-G turn. Lock-on was re- 


acquired in a maximum of 4 time slots. Figure 18 shows the 
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FIGURE 17: X Deviation from True for a Track with 2/3-G 
and 1-G Turns without Noise in the Area of a 


Single Array -- Sequential Routine 
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FIGURE 18: Y Deviation from True for a Track with 2/3-G 
and 1-G Turns without Noise in the Area of a 
Single Array -- Sequential Routine 
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deviation from true in the Y direction with a maximum deviation 
after lock-on for the 1-G turn of under 4 feet. Runs with 

the traditional extended Kalman filter routine showed slightly 
larger deviations with more reaction time required to re-ac- 
quire lock-on. 

In the final tests for this series, zero mean, white Gau- 
ssian noise was added to corrupt the observed transit times. 
Figure 19 depicts the estimate's deviation from true track 
in the X direction using the traditional filter routine. The 
target experienced 1/3-G turns at time slots 25 and 65. Figure 
20 shows the same track in noise with the same filter parame- 
ters when run with the sequential extended Kalman filter rou- 
tine. Comparison shows that the latter had approximately 2 
feet less maximum deviation and that while the traditional 
routine had trouble re-acquiring lock-on after the turn, the 
sequential routine regained track within 4 time slots. Figures 
21 and 22 depict the same type of behavior for deviation in 
the ¥Y direction. Maximum deviation again was slightly less and 
lock-on more efficiently re-acquired using the sequential rou- 
tine, 

In the cases involving more radical maneuvers, the se- 
quential routine continued to perform better against the mo- 
deled noise. Figure 23 shows the estimate's deviation from 
true track in the X direction for a run with a 1-G maneuver at 
time slot 25 and a 2/3-G maneuver at time 65 in noise. This 


graph for the traditional routine shows maximum deviation at 
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FIGURE 19: X Deviation from True for a Track with 1/3-G 
Turns with Noise in the Area of a Single 
Array -- Traditional Routine 
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FIGURE 21: Y Deviation from True for a Track with 1/3-G 
Turns with Noise in the Area of a Single 


Array -- Traditional Routine 
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FIGURE 22: Y Deviation from True for a Track with 1/3-G 
Turns with Noise in the Area of a Single 
Array -- Sequential Routine 
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the 1-G turn of approximately 9 feet and the difficulty the 
filter had in re-acquiring lock-on, Figure 24, which is the 
Same track and filter parameters run with the sequential rou- 
tine, shows a marked improvement in both maximum deviation and 
efficiency in re-acquiring the track after a turn in noise. 
Figures 25 and 26 demonstrate again the better performance 
of the sequential extended filter routine regarding deviation 


in the Y direction for the more radically maneuvering track. 


i onkRLES FOUR 

In the fourth series of tests, the target speeds were in- 
creased to the 40 to 50 Knot range in order to bring the simu- 
lation in line with speeds actually experienced on the range. 
In a noiseless environment, both routines eneved Similar per- 
formance with a maximum deviation from true track during the 
run under 2 feet. With random noise added this deviation in- 
creased and Figure 27 is a plot of the deviation in the X 
direction using the traditional routine. When compared to 
Figure 28 which is the same track using the sequential routine 
with the same parameters, a marked improvement in maximum de- 
viation and Lock-on efficiency is noticeable. Better perfor- 
Mance by the sequential routine was also present regarding 
Bevyiation in the Y direction. 

When Y3-G turns were introduced, the routines performed 
similarly in a noiseless environment, With random noise and 
Maneuvers the routines were again comparable with the tra- 


ditional routine performing slightly better as is shown in 
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FIGURE 23: X Deviation from True for a Track with o/3-G 
and 1-G Turns with Noise in the Area of a 
Single Array -- Traditional Routine 
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FIGURE 24: X Deviation from True for a Track with 2/3-G 
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Single Array -- Sequential Routine 


56 





101.09 


1.00 266 SL. 00 76.00 
FUEL KARAMANK EMR ARKOEM KA, REESE REKELER EM AK KRERY, KEKE ELEY REKKKEEM REN RMR ET RAE ER KREREMRKEM AYE 


4e 73 + o 


+ 
* x 

« ” 

x x 

fe > '. bd 

* . Be 

2 =x 

«x e = 

[4 e a< 

3 x 

3.22 + : : 
« . = 

= = 

= & 

* * 

x x 

x = 

2 ° ° bad 

x “« 

3 ® ° - 

ae 70 us 2 ® e = 
* . é & 

% ° ° Peed 

af = = 

* e . e ~ 

« ‘a ~ 5 * 

* a = e e x 

x e os e = 

* e so ®@ = ¥ 

* ° ° ee e = 

0-15 + ° ° . e bo 
* esta teense e @Besenscaseaesve eevee eseeaeeaespeeeeveecacaaesp eae ea ease seeaoseeeveseeeceaescseeseevesa ee escaev, eee as eva ewan eaeseecaeeaean env a8 & 
RDEV 3 : 
sf a e « es e a 

FT) x : i : se : ; 2 
sd 3 e s e at 

* e e x 

* 2 e ° « * 

* a e * 

2 ‘ x 

“1.33 + 2 + 
* : . « 

* “ 

* ° ° = 

* > . ve 

x = BT 4 

% x 

x e = 

a * 

* 7 x 

-2.34 + + 
* Be 4 

2 x 

*« BJ 

= 5 x 

x * ™ 

x = = 

® 5d 

cai % * 

eI = 

~4.36 + ° “ 


wee ee ee a ee a EE EK CHAE AE EMRE RAERAEAAKAHEAYRKeAKE Ke Tr 


1.090 26-00 1.0 76.00 20L. 00 


TIME SLOTS 


X-SCALE2 “"#"= G.LZ25E OL UNITS 


Y-SCALE: "*%: Q.152E£ OO UNITS 
DEVIATION FROM TRUE X 


MeCunnec, A. Devyaacion from Truce for a Straight Running 
42 Knot Target with Noise in the Area of a 


Single Array -- Traditional Routine 


5) 





1.00 26.00 51.00 . 76.00 191.00 
ES he me ee ge KO KM, RO RK REE, KREEKRAK ERE ACKER EK EKER ROKER E 
Re ccuat 6 + 
3 “« 
™ *« 
* s 
e ¢ 
4 x 
* « 
x bad 
4 be 4 
3 4 
3.23 + + 
* & 
* we 
y € 
t « 
x % 
a = 
* * 
a r 
S m 
1.94 + . * 
x * 
x - ~ 
Lg a = 
a x 
* 4 
= ™ 
* a e = 
bal ee = 
‘ : , re 
O)F 65 + oe @ e e e e + 
* e e i nd 
x e e@a ® e e = 
* e e 2 «x 
x = e e e e ene e « ae ee e e = 
DEV x @eeaneeeoe_ee«e«eeeeaeneer#*entenspeevrnesnseervteeeeeeee#eeese @®eeeseee#serteeste @®#@teeseee @eeeaseest#n#eeeertteste se @®eee@e#seeseete¢ x 
*« e ea @2aeeoeese a a e ae * 
(FT) : ae - : 
x ‘J 2 e = 
« ea ea a i 
—Ce &4 + e e + 
* ° * * 
+s > = 
3 e e ba = 
* e e x 
7 ; « 
* - ad 
+ : 5 * 
x x 
x * 
-~1.53 + + 
x “ € 
= “ 
* A « 
« = 
+ 2 
«x x 
t s i: 
% . = 
x © 
~3.22 + - * 


EE A Mf WO ee eae ee ee ae eee ee Oh 


1.00 26. 00 2b.00 76.00 1031 .Ce 


TIME SLICES 


X-SCALE: "#2 0.125E OL UNITS 


Y-SCALE: “#2 0.129E 00 UNITS 
DEVIATION FROM TRUE X 


FIGURE 28: X Deviation from True for a Straight Running 
42 Knot Target with Noise in the Area of a 


Single Array -- Sequential Routine 
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Figures 29 and 30. Figure 29 depicts the estimate's deviation 
from true in the X direction for the traditional routine and 
Figure 30 for the sequential. In this track the target man- 
euvered, using 1/3-G turns, at time slots 25 and 65. A 
slightly better performance was also exhibited by the tradi- 
mlonal routine in the Y direction. 

In tests in which more radical maneuvers were made, the 
traditional routine again exhibited slightly better performance 
in noise. Figure 31, shows the deviation in the X direction for 
a track with a 1-G turn at time 25 and a 2/3-G turn at 65 using 
the sequential routine. Comparision with Figure 32, which is 
the same track using the traditional routine, indicates that 
the performances were similar with the traditional routine 


having a slight edge. 


E. SERIES FIVE 

In the last series of tests, the targets were tracked 
through multiple arrays using the handoff scheme described in 
the previous seciton. Figure 33 is a geographic plot of a 
typical track including hydrophone positions. Figure 34 de- 
picts the estimates deviation from true in the X direction 
using the traditional routine for a straight running target in 
noise. Figure 35 is the X deviation for the same track using 
the sequential routine. Through the multiple arrays, both 
routines were comparable, and the handoff from array to array 
was smooth with no glitches. Figures 36 and 37 also show com- 
parable results for deviation in the Y direction for this same 
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FIGURE 29: X Deviation from True for a Track with 1/3-G 
Turns at 42 Knots with Noise in the Area of a 
Single Array -- Traditional Routine 
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FIGURE 30: X Deviation from True for a Track with 1/3-G 
Turns at 42 Knots with Noise in the Area of a 


Single Array -- Sequential Routine 
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FIGURE 31: X Deviation from True for a Track with 2/3-G 
and 1-G Turns at 42 Knots with Noise in the 


Area of a Single Array -- Sequential Routine 
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FIGURE 32: X Deviation from True for a Track with 2/3-G 


and 1-G Turns at 42 Knots with Noise in the 


Area of a Single Array -- Traditional Routine 
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FIGURE 35: X Deviation from True for a Straight Running 
Track through Multiple Arrays with Noise -- 


Sequential Routine 
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run. The target was traveling at 40 knots and handoff was 


accomplished at the following times: 


Handoff 1 to 2 --- Time Slot 7/7. 
Handoff 2 to 3 --- Time Slot 168 
Handoff 3 to 4 --- Time Slot 260 
Handoff 4 to 5 --- Time Slot 352 
Handoff 5 to 6 --- Time Slot 443 
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VII. CONCLUSIONS 


Both extended Kalman filter routines designed will provide 
on-line, real time estimates of targets with various maneuvers 
ie to '=-G turns. 

Implementation at the range computer facilities can be 
accomplished by loading the received transit times into a file 
in memory and reading them into the routine with subroutine 
DREAD. The hydrophone positions can also be read into a file 
to be referenced during operation. The initial covariance of 
estimation error P(1/0), read in as a constant matrix, can be 
varied with the uncertainty of the targets initial positions 
and velocities. Any greatly erroneous time measurements caused 
by multipath signals or from noise spikes will cause the gains 
to zero, making the estimated position equal to the predicted, 
mAs oUtting the filter in ‘coast!, and preventing catastrophic 
failure. 

The system was remodeled to include the X and Y velocities 
in the linearizing matrix H. This model is depicted in Figure 
38. With a constant Z, the targets position at time ty is 
mm  % 


070° 
drophone array, the target with X velocity (V,) and Y velocity 


By the time the transmitted signal has reached the hy- 


(V.,) has moved to the position at to 


x A 


II 


op tVyt 


YotVyt 


where T is the transit time of the signal 


Bg 


ai 


70 









Xo X > 
MOVING TARGET in the X-Y PLANE 
X= Xp + VxT 


Y= Yo + VT 


FIGURE 38: Geometry used to remodel the System 
to include X and Y Velocities in the 
iplee Wie eves be. < 





= Z 2 
T = 1/VEL Xo + to 


and VEL is the velocity of propagation of sound in water. 
Including the new positions corresponding to the target 
location when the signal is received gives a new equation for 


the transit time. 


el / c 2 = Z 
T = (X V7) so Cas VT) 


VEL 
SOlvVing tor £ preduces: 


) = ¥ (XV +YV )*= (Vv, 24 ¢ VEL) (x°+¥7+27) 


—_ (XV +YV,, y vy 


2 J 
(Vy, +V,, -VEL) 


Upon testing, this added complexity did not give a correspond- 
ing increase in the quality of the estimate. 

In general, both routines were comparable with the se- 
quential filter having the following advantages: 


1. Less deviation and quicker lock-on time after a 
modest maneuver 


2. With the three-sigma gate utilized a noise spike 
encountered will negate data from only that par- 
ticular hydrophone 


3. A matrix inversion is not necessary. 
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APPENDIX A 


PROGRAM DESCRIPTION AND FEATURES 


Two programs were written to implement the extended Kalman 
mebcece routine Lor torpedo tracking. The first, THEFIV, uti- 
lizes the sequential approach described in section IV-A, and 
the second a traditional matrix inversion approach as descri- 
bed in section IV-B. Both routines use the same utility pro- 
grams and are modularized for ease of implementation. 

l. THEFIV - Sequential Routine 

This program is general in nature and many of the 


parameters of the Kalman routine are variable including: 


a. The number of states in the routine - N 
b. The number of random forcing functions - M 
c. The number of measurements - J 


d. Data rate or sample time - TO 
end. 0 (12) - 0(3,4) 

e. Number of time slots - JTIME 
The constant matrices PHI,R,COVW and GAMMA are read in using 
subroutines in the utility program AUX. The filter is ini- 
tialized with P(1/0) and X(1/0) (initial covariances of esti- 
mation error and states) also using AUX, The first state 
estimate is at time 1 and continues until ITIME = JTIME+1, 
True measurement times (ZI) are read in, four for each time 
slot (TAT Ty Ty), using the subroutine DREAD listed in AUX 


and corrupted by zero-mean, white Gaussian noise using the 
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IBM-360 subroutine SNORM. For each of the four time measure- 
ments the corresponding row of the linearizing H matrix is 
calculated in the utility subroutine CHROW, and the corres- 
ponding gain matrix column GI is found. These row and column 
Values are then utilized in forming the covariance of exti- 
mation error for the particular time measurement PI. Next 

the estimate of the observation time M(X) from that particular 
hydrophone called ZHAT is formed using the subroutine CZHAT 
and the residual ZDIFF = ZI-ZHAT. Finally, the estimate of 
the states XI based on one time measurement is calculated, and 
the process is repeated for the next measurement. After four 
iterations, XI becomes the state estimate and PI becomes the 
updated covariance of estimation error PKK, and the predictions 
of the states and covariances XKKM1 and PKKM1 are formed. 

For testing, this program used the IBM-360 library sub- 
routine PLOTP to obtain plots of the states and covariances 
versus time, estimate deviation from true versus time and a 
geographic track. 

2. THESIS ~ Traditional Routine 

THESIS utilized the same format as THEFIV. The para- 
meters N,M,JS, sample time and number of time slots were still 
variable, and the filter initial conditions and constant 
Matrices were read in by the subroutines located in AUX. True 
time measurements were again read in for each time slot through 
DREAD and corrupted with noise from SNORM. The linearizing H 


matrix was formed row by row using subroutine CHROW and used 
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to calculate the gain matrix G. The symmetric matrix inver- 
sion in the gain equation was done by the IBM-360 library 
Subroutine SINV. The estimate vector of the observations 
ZHAT is formed iteratively by CZHAT and used to calculate the 
residual vector 2DIFE. Next the estimate XKK and updated co- 
Variance of error matrix PKK is calculated once for each time 
slot. Finally, the predictions XKKMl and PKKM1 are formed 
before the process is repeated for the next time slot. PLOTP 
was again utilized for the output graphs. 
3. Utility Programs 

These subroutines were designed to be used for re- 
petitive calculations and processes. The first, AUX performs 
pebadata Input functions ama matrix manipulations including: 

a. PROD - multiplying two matrices 

b. MMULT - multiplying a matrix and a vector 

c. VMULT - multiplying two vectors 


d. MREAD - reading in a matrix 


e. TRANS transposing a matrix 
f. ADD - adding two matrices 
g. VREAD - reading in a vector 


h. DREAD - reads in a matrix containing observed 
time data 


i. TRREAD - reads in a matrix containing true posi- 
tional data for comparison 


The second utility subroutine CZHAT calculates the esti- 


mate of the observation times (TOT Te Or T,) using the 


ees 
% 
predicted state values (X(K/K-1)). 
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The subroutine CHROW calculates a row of the lineariz- 
ing H matrix, each row corresponding to a particular observa- 


tion time measurement. 


A. VARYING STATE TRANSITION MATRIX 
In the Wise model that represents the dynamics of the 
Kalman filter, the State Transition and GAMMA matrices are as 


follows: 


eZ a2 0 0 
t ) 0 
rT = 0 27 2 0 
0 Al 0 
0 0 T 


where T is the sampling time. In the tracking problem, this 
time is not always constant. As is shown in Figure 39 it 
varies with the transit times required from positions in 
adjacent time slots. In Figure 39, the target is at position 
1 at arbitrary time tye and is at position 2, the next time 
slot, at t,+T where T is the time between pings of the target 


0 
Eransducer = 1.31 seconds. The times that the transmitted 
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Moving Target in the X~-Y Plane 


I ee) ar Poor ee) Sa (0 aera, 
Ts = T + (to - Ga) 


FIGURE 39: Geometry used for the Time Varying 
State Transition Matrix 
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StomalsSeare received by ehe hydrophone array are: 


Time signal received from position 1 = t,+t 


Oe 
Time signal received from position 2 = tytT+t. 
where t and t. are the transit times from position l 


and position 2 respectively. The difference between these two 


times signals are received is the sampling time Te: 


rj 
iI 


(t)+T+t.) = (t)tt,) 


Therefore, the sampling time differs each time slot from the 
clocked ping time by the difference in the adjacent slot 
transit times. 

In the programs THEFIV and THESIS, this difference 
is calculated using the average measured times from the four 


hydrophones for each time slot. 


B. ADAPTIVE Q 
The Q matrix which appears in the predicted covariance of 


error equation 

P(K+1/K) = @P(K/K) 0 +Q(K) 
and is formed by 

Q(K) = rcovwr 


is a measure of the amount of target maneuverability that can 
be handled by the filter, If more random excitations (or 


accelerations) by the target is expected, Q is increased which 
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in turn increases the covariances of estimation error P(K+1/K) 
and the filter gains G. Corresponding the filter puts more 
emphasis on the incoming data and is better able to see and 
react to target turns. However, if the filter gains are in- 
creased the filter~bandwidth is widened, which lets in more 
noise, and makes the filter more susceptible to error. The 
adaptive Q routine (Reference 4) varies the Q matrix as the 
velocities in the X and Y directions are increased or de- 
creased. This routine was implemented in the subroutine QFIND 
Weel ampucs Of: 


SIGACC = expected maximum acgeleration in the X or Y 
direction in ft/sec 


SIGDIV = expected maximum acceleration in the Z direction 
in ft/sec 
SIGCC = expected maximum target course change in degrees/ 


sec 


OFIND is listed in Appendix D. 
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SEQUENTIAL EXTENDED KALMAN FILTER 
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APPENDIX C 
TRADITIONAL EXTENDED KALMAN FILTER 
THESIS 
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